/*
* CFile1.c
*
* Created: 17.09.2013 21:51:45
*  Author: maxim
*/
#include "main_func.h"
#include "RGB_LED_USART.h"
#include "TWI_Master.h"
#include "mrtos.h"

int plus(int a, int b) {
	return a+b;
}

inline unsigned int cs_calc(unsigned char b) {
	return b;
}

volatile Type_command USART_command;
volatile unsigned char state_ReceiveLoop;
volatile Type_TWI to_TWI;
unsigned char bufForAnswer[30];

extern unsigned char USART_Error_Buffer_overrun;

// Receive commands by USART
unsigned char task_ReceiveLoop_func(void)
{
	static unsigned char csl,csh,nl,nh;
	static unsigned int cs,cs_need;
	static unsigned char count_in_USART;
	static unsigned char PWM_Values_index;
	static unsigned char tmp;
	//extern volatile Type_command USART_command;
	
	if(USART_Error_Buffer_overrun) {
		state_ReceiveLoop=State_usart_error_while_receiveing;
		USART_send_error(Error_USART_Buffer_overrun);
		USART_Error_Buffer_overrun=0;
	}
	count_in_USART=USART_GetRxCount();
	switch (state_ReceiveLoop) {
		//Init state - not received start byte
		case State_init_state:
		if(count_in_USART && (!USART_command.received))  // if previous command finished by other tasks and USART buffer have received chars
		if(USART_Start_Byte==USART_GetChar()) {    // if received start byte
			mRTOS_SetSystemTime(0);    // system time is used for check timeout while receive
			state_ReceiveLoop=State_usart_start_byte_received;
		}
		break;
		
		//start byte received, next 2 bytes - CS
		case State_usart_start_byte_received:
		if(count_in_USART) {
			csl=USART_GetChar();
			state_ReceiveLoop=State_usart_CS1_received;
		}
		break;
		case State_usart_CS1_received:
		if(count_in_USART) {
			csh=USART_GetChar();
			cs_need=((unsigned int)csh<<8) | ((unsigned int)csl); //CS received from Master
			cs=0;                                                 //CS calculated
			state_ReceiveLoop=State_usart_CS2_received;
		}
		break;
		
		//CS received, next byte - command code
		case State_usart_CS2_received:
		if(count_in_USART) {
			USART_command.command=USART_GetChar();
			cs+=cs_calc(USART_command.command); // adding command code to CS
			state_ReceiveLoop=State_usart_command_received;
		}
		break;
		
		//Command is received, the following 2 bytes - number of bytes of parameters
		case State_usart_command_received:
		if(count_in_USART) {
			nl=USART_GetChar();    //receiving low byte
			cs+=cs_calc(nl);
			state_ReceiveLoop=State_usart_num_of_bytes1_received;
		}
		break;
		case State_usart_num_of_bytes1_received:
		if(count_in_USART) {
			nh=USART_GetChar();    //receiving Hight byte
			cs+=cs_calc(nh);
			USART_command.params_count=((unsigned int)nh<<8)|((unsigned int)nl);
			if(USART_command.params_count>USART_command_max_params) {   //if received num of params more than max params, then error while USART receiving
				state_ReceiveLoop=State_usart_error_while_receiveing;
				USART_send_error(Error_USART_too_many_params);
				} else {
				if(USART_command.params_count) {   // if params_count > 0 go to receiveing params
					state_ReceiveLoop=State_usart_num_of_bytes2_received;
					PWM_Values_index=0;
					} else {             // else all data received
					state_ReceiveLoop=State_usart_all_data_received;
				}
			}
		}
		break;
		case State_usart_num_of_bytes2_received:
		//receiving params to temp buf
		if(count_in_USART) {
			tmp=USART_GetChar();             //get byte
			cs+=cs_calc(tmp);                //and calc check summ
			USART_command.params[PWM_Values_index++]=tmp;
			if(PWM_Values_index>=USART_command.params_count) {  //if all params received
				state_ReceiveLoop=State_usart_all_data_received;
			}
		}
		break;
		
		case State_usart_all_data_received:
		if(cs!=cs_need) {                         //if CS not match
			state_ReceiveLoop=State_usart_error_while_receiveing;  //send error message
			USART_send_error(Error_USART_CS);
			} else {
			state_ReceiveLoop=State_init_state;
			USART_command.received=TRUE;
			//USART_send_ok();
			//mRTOS_TASK_ACTIVE()  //start task task_CommanderLoop
		}
		break;
		case State_usart_error_while_receiveing:
		state_ReceiveLoop=State_init_state;     //if error while receiving
		USART_FlushRxBuf();         //clean receiver buffer
		sei();
		break;
	}
	if((state_ReceiveLoop!=State_init_state) || count_in_USART) {
		if((state_ReceiveLoop!=State_init_state) && (mRTOS_GetSystemTime()>USART_TIMEOUT)) {
			state_ReceiveLoop=State_usart_error_while_receiveing;
			USART_send_error(Error_USART_TIMEOUT);
		}
		tmp=TRUE;
		
		} else {
		tmp=FALSE;
	}
	return tmp;
}



inline void Command3()
{
	to_TWI.string[0]=PCA_addr<<1;
	to_TWI.string[1]=USART_command.params[0]+PCA_first_channel;
	to_TWI.string[2]=USART_command.params[1];
	to_TWI.count=3;
	to_TWI.ready=TRUE;
	USART_command.received=FALSE;
}

inline void Command5()
{
	unsigned char tmp;
	to_TWI.string[0]=PCA_addr<<1;
	to_TWI.string[1]=0b10000000;
	to_TWI.string[2]=0b10000001;
	to_TWI.string[3]=0b00000101;
	for(tmp=0;tmp<16;tmp++) {
		to_TWI.string[tmp+4]=USART_command.params[tmp];
	}
	to_TWI.string[20]=0b11111111; //GRPPWM
	to_TWI.string[21]=0b00000000; //GRPFREQ
	to_TWI.string[22]=0b10101010; //LEDOUT0
	to_TWI.count=23;
	to_TWI.ready=TRUE;
	USART_command.received=FALSE;
}

inline void Command10()
{
	//USART_SendStr("1");
	//USART_SendStr(VERSION);
	bufForAnswer[0]=0x31;
	bufForAnswer[1]=0;
	bufForAnswer[2]=0;
	bufForAnswer[3]=0;
	bufForAnswer[4]=1;
	USART_send_result(5);
	USART_command.received=FALSE;
}

inline void Command11()
{
	to_TWI.string[0]=PCA_addr<<1;
	to_TWI.string[1]=0b10000100;
	to_TWI.count=2;
	to_TWI.ready=TRUE;
	USART_command.command=12;
	USART_command.received=TRUE;
}

inline void Command12()
{
	to_TWI.string[0]=(PCA_addr<<1)|1;
	to_TWI.count=6;
	to_TWI.ready=TRUE;
	USART_command.received=TRUE;
	USART_command.command=13;
}

inline void Command13()
{
	unsigned char tmp;
	if(TWI_Get_Data_From_Transceiver(to_TWI.string,6)) {
		for(tmp=0;tmp<6;tmp++) {
			USART_PutChar(to_TWI.string[tmp]);
		}
		} else {
		USART_send_error(Error_TWI);
	}
	USART_command.received=FALSE;
}



void task_CommanderLoop_func(void) {
	if(USART_command.received && (!to_TWI.ready)) {
		switch(USART_command.command) {
			case 1:
			break;
			case 2:
			break;
			case 3:
			Command3();
			break;
			case 4:
			break;
			case 5:
			Command5();
			break;
			case 6:
			break;
			case 10:
			Command10();
			break;
			case 11:  //request data from TWI
			Command11();
			break;
			case 12:  //
			Command12();
			break;
			case 13: //receiving data from TWI
			Command13();
			break;
			default:
			break;
		}
		//USART_command.received=FALSE;
	}
}



#define SEND_DATA 1
#define WAIT_TRANSMIT 2

unsigned char task_SendLoop_func(unsigned char operation)
{
	if(to_TWI.ready) {
		switch(operation) {
			case SEND_DATA:
			if(!TWI_Transceiver_Busy()) {
				TWI_Start_Transceiver_With_Data(to_TWI.string,to_TWI.count);
				operation=WAIT_TRANSMIT;
			}
			break;
			case WAIT_TRANSMIT:
			if(TWI_Transceiver_Busy())
			{
				// wait
			} else
			{
				if(TWI_statusReg.lastTransOK)
				{
					USART_send_ok();
				} else
				{
					USART_send_error(Error_TWI);
				}
				operation=SEND_DATA;
				to_TWI.ready=FALSE;
			}
			break;
		}
	}
	return operation;
}

/*void debug_command()
{
USART_PutChar(USART_command.command);
//USART_SendStr("\r\n");
USART_PutChar(USART_command.params_count);
//USART_SendStr("\r\n");
USART_PutChar(USART_command.received);
//USART_SendStr("\r\n");
unsigned char i;
for(i=0;i<USART_command.params_count;i++) {
USART_PutChar(USART_command.params[i]);
}
//USART_SendStr("\r\n");
}*/

void task_ReceiveLoop(void) {
	state_ReceiveLoop=State_init_state;
	while(1) {
		if(task_ReceiveLoop_func()) {
			mRTOS_DISPATCH;
		} else {
			mRTOS_TASK_WAIT(1);
		}
		
	}
}

void task_CommanderLoop(void) {
	to_TWI.count=0;
	to_TWI.ready=FALSE;
	while(1) {
		task_CommanderLoop_func();
		mRTOS_TASK_WAIT(1);
	}
}

void task_SendLoop( void ) {
	static unsigned char oper;
	oper=SEND_DATA;
	while(1) {
		oper=task_SendLoop_func(oper);
		if(oper==WAIT_TRANSMIT) {
			mRTOS_DISPATCH;
			} else {
			mRTOS_TASK_WAIT(1);
		}
	}
}


void USART_send_error(unsigned char err) {
	//USART_PutChar(0x30);
	//USART_PutChar(err);
	bufForAnswer[0]=0x30;
	bufForAnswer[1]=err;
	USART_send_result(2);
}

void USART_send_ok()
{
	//USART_PutChar(0x31);
	bufForAnswer[0]=0x31;
	USART_send_result(1);
}


void USART_send_result(unsigned char len) {
	//buffer[] structure:
	//1st byte - OK (0x31, 0x30, etc)
	//other bytes - info bytes about result operation
	unsigned int cs=cs_calc(bufForAnswer[0]);
	unsigned char i;
	
	cs+=cs_calc(len-1);
	for(i=1;i<len;i++) {
		cs+=cs_calc(bufForAnswer[i]);
	}
	USART_PutChar(0x66);
	USART_PutChar((unsigned char)(cs & 0b0000000011111111));
	USART_PutChar((unsigned char)(cs>>8));
	USART_PutChar((bufForAnswer[0]));
	USART_PutChar(len-1);
	USART_PutChar(0);
	for(i=1;i<len;i++) {
		USART_PutChar(bufForAnswer[i]);
	}
}